/*
 * servoS7test.c
 *
 *  Created on: 2025年1月1日
 *      Author: chenlenan
 */
#include "servoS7test.h"
#include <stdio.h>
#include "osal.h"
#include "ethercat.h"
#include "slaveinfo.h"

extern char IOmap[4096];

extern int dorun;
extern int32 cur_pos;
extern uint8 cur_mode;
S7_PDO_Output *outputS7;
S7_PDO_Input *inputS7;
uint8 S7flag;

int ServoS7setup(uint16 slave)
{
	int retval;
	uint16 u16val;
	uint8  u8val;
	uint32 u32val;
	retval = 0;

	u8val = 0;
	retval += ec_SDOwrite(slave, 0x1c12, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);
	u16val = 0x1600;
	retval += ec_SDOwrite(slave, 0x1c12, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTRXM);
	u8val = 1;
	retval += ec_SDOwrite(slave, 0x1c12, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);


	u8val = 0;
	retval += ec_SDOwrite(slave, 0x1600, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);
	u32val = 0x60400010;
	retval += ec_SDOwrite(slave, 0x1600, 0x01, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);
	u32val = 0x607A0020;
	retval += ec_SDOwrite(slave, 0x1600, 0x02, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);
	u32val = 0x60600008;
	retval += ec_SDOwrite(slave, 0x1600, 0x03, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);
	u32val = 0x5FFE0008;
	retval += ec_SDOwrite(slave, 0x1600, 0x04, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);
	u8val = 4;
	retval += ec_SDOwrite(slave, 0x1600, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);


	u8val = 0;
	retval += ec_SDOwrite(slave, 0x1c13, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);
	u16val = 0x1a00;
	retval += ec_SDOwrite(slave, 0x1c13, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTRXM);
	u8val = 1;
	retval += ec_SDOwrite(slave, 0x1c13, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);

	u8val = 0;
	retval += ec_SDOwrite(slave, 0x1A00, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);
	u32val = 0x60410010;
	retval += ec_SDOwrite(slave, 0x1A00, 0x01, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);
	u32val = 0x60640020;
	retval += ec_SDOwrite(slave, 0x1A00, 0x02, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);
	u32val = 0x606C0020;
	retval += ec_SDOwrite(slave, 0x1A00, 0x03, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);
	u32val = 0x603F0010;
	retval += ec_SDOwrite(slave, 0x1A00, 0x04, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);
	u32val = 0x60610008;
	retval += ec_SDOwrite(slave, 0x1A00, 0x05, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);
	u32val = 0x5FFF0008;
	retval += ec_SDOwrite(slave, 0x1A00, 0x06, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);
	u8val = 6;
	retval += ec_SDOwrite(slave, 0x1A00, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);


	return 1;
}

void servoS7test(char *ifname)
{
	int cnt, i, j;
	uint16 ssigen;

	printf("Starting Servo test\n");

	/* initialise SOEM, bind socket to ifname */
	if (ec_init(ifname))
	{
		printf("ec_init on %s succeeded.\n",ifname);
		/* find and auto-config slaves */
		if ( ec_config_init(FALSE) > 0 )
		{
			dorun = 0;
			S7flag = 1;
			printf("%d slaves found and configured.\n",ec_slavecount);

			/* read indevidual slave state and store in ec_slave[] */
			ec_readstate();
			for(cnt = 1; cnt <= ec_slavecount ; cnt++)
			{
				printf("ec_slave[%d].eep_man = %x\n",cnt,ec_slave[cnt].eep_man);
				printf("ec_slave[%d].eep_id = %x\n",cnt,ec_slave[cnt].eep_id);
				/* check for EL2004 or EL2008 */
				if((ec_slave[cnt].eep_id == 0x00000748) || (ec_slave[cnt].eep_id == 0x00000006))
				{
					ec_slave[cnt].PO2SOconfig = &ServoS7setup;
				}
			}


			ec_config_map(&IOmap);
			/* configure DC options for every DC capable slave found in the list */
			ec_configdc();
			ec_dcsync0(1, TRUE, 1000000, 250000); // SYNC0 on slave 1


			/* wait for all slaves to reach SAFE_OP state */
			ec_statecheck(0, EC_STATE_SAFE_OP,  EC_TIMEOUTSTATE);
			do
			{
				ec_send_processdata();
				ec_receive_processdata(EC_TIMEOUTRET);
				ec_statecheck(0, EC_STATE_SAFE_OP, 50000);
				ec_statecheck(1, EC_STATE_SAFE_OP, 50000);
			}
			while ((ec_slave[0].state != EC_STATE_SAFE_OP)&&(ec_slave[1].state != EC_STATE_SAFE_OP));
			printf("Slave 0 State=0x%04x\r\n",ec_slave[0].state);
			printf("Slave 1 State=0x%04x\r\n",ec_slave[1].state);

			osal_usleep(100000);
			int retval;
			uint16 u16val;
			uint8  u8val;
			u8val = 8;
			retval += ec_SDOwrite(1, 0x6060, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);
			u16val = 0x80;
			retval += ec_SDOwrite(1, 0x6040, 0x00, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTRXM);


			/* Print som information on the mapped network */
			for( cnt = 1 ; cnt <= ec_slavecount ; cnt++)
			{
				printf("\nSlave:%d\n Name:%s\n Output size: %dbits\n Input size: %dbits\n State: %d\n Delay: %d[ns]\n Has DC: %d\n",
											cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
											ec_slave[cnt].state, ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
				printf(" Configured address: %x\n", ec_slave[cnt].configadr);
				printf(" Outputs address: %x\n", ec_slave[cnt].outputs);
				printf(" Inputs address: %x\n", ec_slave[cnt].inputs);

				for(j = 0 ; j < ec_slave[cnt].FMMUunused ; j++)
				{
					printf(" FMMU%1d Ls:%x Ll:%4d Lsb:%d Leb:%d Ps:%x Psb:%d Ty:%x Act:%x\n", j,
									(int)ec_slave[cnt].FMMU[j].LogStart, ec_slave[cnt].FMMU[j].LogLength, ec_slave[cnt].FMMU[j].LogStartbit,
									ec_slave[cnt].FMMU[j].LogEndbit, ec_slave[cnt].FMMU[j].PhysStart, ec_slave[cnt].FMMU[j].PhysStartBit,
									ec_slave[cnt].FMMU[j].FMMUtype, ec_slave[cnt].FMMU[j].FMMUactive);
				}
				printf(" FMMUfunc 0:%d 1:%d 2:%d 3:%d\n",ec_slave[cnt].FMMU0func, ec_slave[cnt].FMMU1func, ec_slave[cnt].FMMU2func, ec_slave[cnt].FMMU3func);
				printf(" MBX length wr: %d rd: %d MBX protocols : %2.2x\n", ec_slave[cnt].mbx_l, ec_slave[cnt].mbx_rl, ec_slave[cnt].mbx_proto);
				ssigen = ec_siifind(cnt, ECT_SII_GENERAL);
				/* SII general section */
				if (ssigen)
				{
				   ec_slave[cnt].CoEdetails = ec_siigetbyte(cnt, ssigen + 0x07);
				   ec_slave[cnt].FoEdetails = ec_siigetbyte(cnt, ssigen + 0x08);
				   ec_slave[cnt].EoEdetails = ec_siigetbyte(cnt, ssigen + 0x09);
				   ec_slave[cnt].SoEdetails = ec_siigetbyte(cnt, ssigen + 0x0a);
				   if((ec_siigetbyte(cnt, ssigen + 0x0d) & 0x02) > 0)
				   {
					  ec_slave[cnt].blockLRW = 1;
					  ec_slave[0].blockLRW++;
				   }
				   ec_slave[cnt].Ebuscurrent = ec_siigetbyte(cnt, ssigen + 0x0e);
				   ec_slave[cnt].Ebuscurrent += ec_siigetbyte(cnt, ssigen + 0x0f) << 8;
				   ec_slave[0].Ebuscurrent += ec_slave[cnt].Ebuscurrent;
				}
				printf(" CoE details: %2.2x FoE details: %2.2x EoE details: %2.2x SoE details: %2.2x\n",
						ec_slave[cnt].CoEdetails, ec_slave[cnt].FoEdetails, ec_slave[cnt].EoEdetails, ec_slave[cnt].SoEdetails);
				printf(" Ebus current: %d[mA]\n only LRD/LWR:%d\n",
						ec_slave[cnt].Ebuscurrent, ec_slave[cnt].blockLRW);
				if ((ec_slave[cnt].mbx_proto & ECT_MBXPROT_COE) && 1)
						si_sdo(cnt);
				if(1)
				{
						if (ec_slave[cnt].mbx_proto & ECT_MBXPROT_COE)
							si_map_sdo(cnt);
						else
							si_map_sii(cnt);
				}
			}

			printf("Request operational state for all slaves\n");
			ec_slave[0].state = EC_STATE_OPERATIONAL;
			/* send one valid process data to make outputs in slaves happy*/
			ec_send_processdata();
			ec_receive_processdata(EC_TIMEOUTRET);


			/* request OP state for all slaves */
			ec_writestate(0);
			/* wait for all slaves to reach OP state */
			ec_statecheck(0, EC_STATE_OPERATIONAL,  5 * EC_TIMEOUTSTATE);
			if (ec_slave[0].state == EC_STATE_OPERATIONAL )
			{
				printf("Operational state reached for all slaves.\n");
				dorun = 1;/* activate cyclic process data */
				outputS7 = (S7_PDO_Output *)ec_slave[1].outputs;
				inputS7  = (S7_PDO_Input *)ec_slave[1].inputs;
			}
			else
			{
				printf("Not all slaves reached operational state.\n");
				ec_readstate();
				for(i = 1; i<=ec_slavecount ; i++)
				{
					if(ec_slave[i].state != EC_STATE_OPERATIONAL)
					{
						printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
								i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
					}
				}
			}

		}
		else
		{
		printf("No slaves found!\n");
		}

	}
	else
	{
		printf("No socket connection on %s\nExcecute as root\n",ifname);
	}
}

int32 acc_cnt;
void servoS7_loop(void)
{
	ec_receive_processdata(EC_TIMEOUTRET);

	uint16 cur_status;

	cur_status = inputS7->StatusWord;//0x6041
	cur_pos = inputS7->CurrentPosition;//0x6064

	if(S7flag == 1){
		if((cur_status & 0x004f) == 0x0040)
		{
			outputS7->ControlWord = 0x06;//0x6040
			//printf("0x06\n");
		}
		else if((cur_status & 0x006f) == 0x0021)
		{
			outputS7->ControlWord = 0x07;
			outputS7->TargetPos = cur_pos;//0x607A
			//printf("0x07\n");
		}
		else if((cur_status & 0x006f) == 0x023)
		{
			outputS7->ControlWord = 0x0F;
			outputS7->TargetPos = cur_pos;//0x607A
			//printf("0x0F\n");
		}
		else if((cur_status & 0x006f) == 0x0027)
		{
			outputS7->ControlWord = 0x1F;
			//printf("0x1F\n");

			outputS7->TargetPos = cur_pos;//0x607A
			S7flag = 2;
			acc_cnt = 1;
		}
	}

	if(S7flag == 2){

		if(acc_cnt < 16000)
		{
			outputS7->TargetPos = cur_pos + acc_cnt*2;//0x607A
		}
		else
		{
			S7flag = 3;
			printf("acc over\n");
		}

		outputS7->ControlWord = 0x1F;
		acc_cnt++;
	}

	if(S7flag == 3){
		outputS7->TargetPos = cur_pos + 32000;//0x607A
		outputS7->ControlWord = 0x1F;
	}

	outputS7->TargetMode = 0x8;
	ec_send_processdata();
}



